package org.pi.common.vision.tool

import org.pi.common.matrix.FloatMatrix
import org.pi.common.matrix.Api._

trait MedianFilterToolTrait {

	def medfilt2d(in:FloatMatrix, fsizex: Int, fsizey: Int): FloatMatrix = medfilt2d(in, fsizex, fsizey, null) 
	def medfilt2d(in:FloatMatrix, fsizex: Int, fsizey: Int , outParam: FloatMatrix): FloatMatrix = quantilfilt2d(in, fsizex, fsizey, fsizex*fsizey/2, outParam) 

	def quantilfilt2d(in:FloatMatrix, fsizex: Int, fsizey: Int, selector:Int): FloatMatrix = quantilfilt2d(in, fsizex, fsizey, selector, null)
	def quantilfilt2d(in:FloatMatrix, fsizex: Int, fsizey: Int, selector:Int , outParam: FloatMatrix): FloatMatrix = {
		var out = outParam;
		if (out==null) out = new FloatMatrix(in.shape :_*);
		if (out.hasSameShape(in)) out = new FloatMatrix( in.shape :_* )

		var x: Int=0
		var y: Int=0
		var fx: Int=0
		var fy: Int=0
		var pos:Int=0
		var pos1:Int =0
		var fpos: Int =0
		var x1:Int =0
		var x2:Int=0
		var y1:Int=0
		var y2:Int=0
		val w = in.length(1)
		val h = in.length(0)
		var array = new Array[Float](1);
		
		var fsizex2: Int = Math.floor(fsizex.toDouble/2.0).toInt;
		var fsizey2:Int = Math.floor(fsizey.toDouble/2.0).toInt;
		
		pos=0;
		x=0;
		while(x<w) {
			y=0;
			while(y<h){
				x1 = if(x>=fsizex2) 0 else (fsizex2-x);
				x2 = if(w-x>fsizex-fsizex2) fsizex else (w-x+fsizex2);
				y1 = if(y>=fsizey2) 0 else (fsizey2-y);
				y2 = if(h-y>fsizey-fsizey2) fsizey else (h-y+fsizey2);
				val numElems = (y2-y1)*(x2-x1);
				if (numElems != array.length) array = new Array[Float](numElems);
				fx=x1
				fpos = 0;
				while(fx<x2) {
					fy=y1;
					pos1 = (y+fy-fsizey2)+(x+fx-fsizex2)*h;
					while(fy<y2) {
						array(fpos)=in(pos1)
						fy+=1
						fpos+=1
						pos1+=1
					}
					fx+=1
				}
				
				scala.util.Sorting.quickSort(array)
				
				if (numElems!=fsizex*fsizey) {
					var selector2 = selector-(fsizex*fsizey-numElems)/2;
					if (selector2<0) selector2=0
					if (selector2>=numElems) selector2=numElems-1
					out(pos) = array(selector2);
				}
				else {
					out(pos) = array(selector);
				}
				y+=1
				pos+=1
			}
			x+=1;
		}

		out;
	}
}

object MedianFilterTool extends MedianFilterToolTrait {

	def main(args: Array[String]) : Unit =  {
		println("main medfilt2d start")
		import org.pi.common.vision.Api.imread
		import org.pi.common.vision.Api.imwrite
		import org.pi.common.vision.Api.rgb2gray
		import org.pi.common.time.Api._

		var C = imread("data/4Spiele.png")
		var G = rgb2gray(C)
		tic
		var LG = medfilt2d(G,9,9)
		toc("medfilt2d")
		imwrite("gray.png",G)
		imwrite("gray_filtered_median.png",LG)
		
		println("main medfilt2d end")
	}

}
